{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys \n",
    "sys.path.append('../scripts/')\n",
    "from kf import *   #誤差楕円を描くのに利用"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_ax(): #axisの準備\n",
    "    fig = plt.figure(figsize=(4,4))\n",
    "    ax = fig.add_subplot(111)\n",
    "    ax.set_aspect('equal')\n",
    "    ax.set_xlim(-5,5)                  \n",
    "    ax.set_ylim(-5,5) \n",
    "    ax.set_xlabel(\"X\",fontsize=10) \n",
    "    ax.set_ylabel(\"Y\",fontsize=10)  \n",
    "    return ax\n",
    "\n",
    "def draw_trajectory(xs, ax): #軌跡の描画\n",
    "    poses = [xs[s] for s in range(len(xs))]\n",
    "    ax.scatter([e[0] for e in poses], [e[1] for e in poses], s=5, marker=\".\", color=\"black\")\n",
    "    ax.plot([e[0] for e in poses], [e[1] for e in poses], linewidth=0.5, color=\"black\")\n",
    "    \n",
    "def draw_observations(xs, zlist, ax): #センサ値の描画\n",
    "    for s in range(len(xs)):\n",
    "        if s not in zlist:\n",
    "            continue\n",
    "            \n",
    "        for obs in zlist[s]:\n",
    "            x, y, theta = xs[s]\n",
    "            ell, phi = obs[1][0], obs[1][1]\n",
    "            mx = x + ell*math.cos(theta + phi)\n",
    "            my = y + ell*math.sin(theta + phi)\n",
    "            ax.plot([x,mx], [y,my], color=\"pink\", alpha=0.5)\n",
    "            \n",
    "def draw_edges(edges, ax):\n",
    "    for e in edges:\n",
    "        ax.plot([e.x1[0], e.x2[0]], [e.x1[1] ,e.x2[1]], color=\"red\", alpha=0.5)\n",
    "        \n",
    "def draw_landmarks(ms, ax): \n",
    "    ax.scatter([ms[k][0] for k in ms], [ms[k][1] for k in ms], s=100, marker=\"*\", color=\"blue\", zorder=100)\n",
    "    \n",
    "def draw(xs, zlist, edges, ms={}):  #ms追加\n",
    "    ax = make_ax()\n",
    "    draw_observations(xs, zlist, ax)\n",
    "    draw_trajectory(xs, ax)\n",
    "    draw_landmarks(ms, ax)  #追加\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_data(): ###graphbasedslam_2d_sensor_readdata\n",
    "    hat_xs = {} \n",
    "    zlist = {} \n",
    "    delta = 0.0\n",
    "    us = {}\n",
    "\n",
    "    with open(\"log2.txt\") as f: #log2.txtに変えておく\n",
    "        for line in f.readlines():\n",
    "            tmp = line.rstrip().split()\n",
    "\n",
    "            step = int(tmp[1])\n",
    "            if tmp[0] == \"x\": \n",
    "                hat_xs[step] = np.array([float(tmp[2]), float(tmp[3]), float(tmp[4])]).T\n",
    "            elif tmp[0] == \"z\": \n",
    "                if step not in zlist: \n",
    "                    zlist[step] = []\n",
    "                zlist[step].append((int(tmp[2]), np.array([float(tmp[3]), float(tmp[4])]).T)) #変更。ψを読み込まないように\n",
    "            elif tmp[0] == \"delta\": \n",
    "                delta = float(tmp[1])\n",
    "            elif tmp[0] == \"u\":\n",
    "                us[step] = np.array([float(tmp[2]), float(tmp[3])]).T \n",
    "                \n",
    "        return hat_xs, zlist, us, delta #us, deltaも返す"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ObsEdge:   ###graphbasedslam_2d_sensor_obsedge\n",
    "    def __init__(self, t1, t2, z1, z2, xs, sensor_noise_rate=[0.14, 0.05]):  #ψの標準偏差を削除\n",
    "        assert z1[0] == z2[0] \n",
    "\n",
    "        self.t1, self.t2 = t1, t2  \n",
    "        self.x1, self.x2 = xs[t1], xs[t2]\n",
    "        self.z1, self.z2 = z1[1], z2[1]\n",
    "        \n",
    "        s1 = math.sin(self.x1[2] + self.z1[1]) \n",
    "        c1 = math.cos(self.x1[2] + self.z1[1])\n",
    "        s2 = math.sin(self.x2[2] + self.z2[1])\n",
    "        c2 = math.cos(self.x2[2] + self.z2[1])\n",
    "\n",
    "        ##誤差の計算##\n",
    "        hat_e = self.x2[0:2] - self.x1[0:2] + np.array([       #self.x2とself.x1は上の2行だけを使う\n",
    "            self.z2[0]*c2 - self.z1[0]*c1, \n",
    "            self.z2[0]*s2 - self.z1[0]*s1\n",
    "        ])                                                                   #ψに関する行列の行と、正規化していた行を削除。\n",
    "            \n",
    "        ##精度行列の作成## \n",
    "        Q1 = np.diag([(self.z1[0]*sensor_noise_rate[0])**2, sensor_noise_rate[1]**2]) #ψの分散を削除\n",
    "        R1 = - np.array([[c1, -self.z1[0]*s1],\n",
    "                                     [s1,  self.z1[0]*c1]])    #3行目、3列目を削除\n",
    "        \n",
    "        Q2 = np.diag([(self.z2[0]*sensor_noise_rate[0])**2, sensor_noise_rate[1]**2]) #ψの分散を削除\n",
    "        R2 = np.array([[c2, -self.z2[0]*s2],\n",
    "                                  [s2,  self.z2[0]*c2]])    #3行目、3列目を削除\n",
    "        \n",
    "        Sigma = R1.dot(Q1).dot(R1.T) + R2.dot(Q2).dot(R2.T)\n",
    "        Omega = np.linalg.inv(Sigma)                                                  #2x2行列になる\n",
    "        \n",
    "        ##大きな精度行列と係数ベクトルの各部分を計算##\n",
    "        B1 = - np.array([[1, 0, -self.z1[0]*s1],\n",
    "                                    [0, 1, self.z1[0]*c1]])                                  #3行目を削除\n",
    "        B2 = np.array([[1, 0,  -self.z2[0]*s2],\n",
    "                                   [0, 1,   self.z2[0]*c2]])                                  #3行目を削除\n",
    "        \n",
    "        self.omega_upperleft = B1.T.dot(Omega).dot(B1)         #ここは計算すると3x3行列のままになる\n",
    "        self.omega_upperright = B1.T.dot(Omega).dot(B2)\n",
    "        self.omega_bottomleft = B2.T.dot(Omega).dot(B1)\n",
    "        self.omega_bottomright = B2.T.dot(Omega).dot(B2)\n",
    "        \n",
    "        self.xi_upper = - B1.T.dot(Omega).dot(hat_e)         #ここも計算すると3次元縦ベクトルのままになる\n",
    "        self.xi_bottom = - B2.T.dot(Omega).dot(hat_e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MotionEdge:\n",
    "    def __init__(self, t1, t2, xs, us, delta, motion_noise_stds={\"nn\":0.19, \"no\":0.001, \"on\":0.13, \"oo\":0.2}):\n",
    "        self.t1, self.t2 = t1, t2                   #時刻の記録\n",
    "        self.hat_x1, self.hat_x2 = xs[t1], xs[t2]    #各時刻の姿勢\n",
    "\n",
    "        nu, omega = us[t2]\n",
    "        if abs(omega) < 1e-5: omega = 1e-5 #ゼロにすると式が変わるので避ける\n",
    "\n",
    "        M = matM(nu, omega, delta, motion_noise_stds)\n",
    "        A = matA(nu, omega, delta, self.hat_x1[2])\n",
    "        F = matF(nu, omega, delta, self.hat_x1[2])\n",
    "        \n",
    "        self.Omega = np.linalg.inv(A.dot(M).dot(A.T) + np.eye(3)*0.0001) #標準偏差0.01の雑音を足す\n",
    "        \n",
    "        self.omega_upperleft = F.T.dot(self.Omega).dot(F)\n",
    "        self.omega_upperright = -F.T.dot(self.Omega)\n",
    "        self.omega_bottomleft = - self.Omega.dot(F)\n",
    "        self.omega_bottomright = self.Omega\n",
    "        \n",
    "        x2 = IdealRobot.state_transition(nu, omega, delta, self.hat_x1)\n",
    "        self.xi_upper = F.T.dot(self.Omega).dot(self.hat_x2 - x2)\n",
    "        self.xi_bottom = -self.Omega.dot(self.hat_x2 - x2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "import itertools \n",
    "def make_edges(hat_xs, zlist):\n",
    "    landmark_keys_zlist = {}\n",
    "\n",
    "    for step in zlist: \n",
    "        for z in zlist[step]:\n",
    "            landmark_id = z[0]\n",
    "            if landmark_id not in landmark_keys_zlist: \n",
    "                landmark_keys_zlist[landmark_id] = []\n",
    "\n",
    "            landmark_keys_zlist[landmark_id].append((step, z))\n",
    "    \n",
    "    edges = []\n",
    "    for landmark_id in landmark_keys_zlist:\n",
    "        step_pairs = list(itertools.combinations(landmark_keys_zlist[landmark_id], 2))\n",
    "        edges += [ObsEdge(xz1[0], xz2[0], xz1[1], xz2[1], hat_xs) for xz1, xz2 in step_pairs]\n",
    "        \n",
    "    return edges, landmark_keys_zlist #ランドマークをキーにしたリストlandmark_keys_zlistも返す"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_edge(edge, Omega, xi): \n",
    "    f1, f2 = edge.t1*3, edge.t2*3\n",
    "    t1 ,t2 = f1 + 3, f2 + 3\n",
    "    Omega[f1:t1, f1:t1] += edge.omega_upperleft\n",
    "    Omega[f1:t1, f2:t2] += edge.omega_upperright\n",
    "    Omega[f2:t2, f1:t1] += edge.omega_bottomleft\n",
    "    Omega[f2:t2, f2:t2] += edge.omega_bottomright\n",
    "    xi[f1:t1] += edge.xi_upper\n",
    "    xi[f2:t2] += edge.xi_bottom"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1回目の繰り返し: 13.339392352057338\n",
      "2回目の繰り返し: 7.661718879214364\n",
      "3回目の繰り返し: 3.397419760080383\n",
      "4回目の繰り返し: 3.6312169310226174\n",
      "5回目の繰り返し: 2.8579570101141036\n",
      "6回目の繰り返し: 2.6648824513716205\n",
      "7回目の繰り返し: 2.3632499162121037\n",
      "8回目の繰り返し: 2.1744191947821734\n",
      "9回目の繰り返し: 2.000489916210123\n",
      "10回目の繰り返し: 1.8635834359128987\n",
      "11回目の繰り返し: 1.7467203897693486\n",
      "12回目の繰り返し: 1.6490794291884514\n",
      "13回目の繰り返し: 1.5659097978082592\n",
      "14回目の繰り返し: 1.4950362495254923\n",
      "15回目の繰り返し: 1.4342774381200105\n",
      "16回目の繰り返し: 1.382101458501944\n",
      "17回目の繰り返し: 1.3372379589502619\n",
      "18回目の繰り返し: 1.298693562617502\n",
      "19回目の繰り返し: 1.2656619882668807\n",
      "20回目の繰り返し: 1.2374924044185334\n",
      "21回目の繰り返し: 1.2136539760189065\n",
      "22回目の繰り返し: 1.1937131428369847\n",
      "23回目の繰り返し: 1.1773149373214282\n",
      "24回目の繰り返し: 1.16416909307157\n",
      "25回目の繰り返し: 1.1540391402125711\n",
      "26回目の繰り返し: 1.1467339787319084\n",
      "27回目の繰り返し: 1.1421012902902652\n",
      "28回目の繰り返し: 1.140022408618522\n",
      "29回目の繰り返し: 1.1404083367113662\n",
      "30回目の繰り返し: 1.1431966888162113\n",
      "31回目の繰り返し: 1.148349388188342\n",
      "32回目の繰り返し: 1.1558509956430336\n",
      "33回目の繰り返し: 1.1657075759372544\n",
      "34回目の繰り返し: 1.1779460338981862\n",
      "35回目の繰り返し: 1.1926138712722039\n",
      "36回目の繰り返し: 1.209779329786074\n",
      "37回目の繰り返し: 1.2295318965337434\n",
      "38回目の繰り返し: 1.2519831543743203\n",
      "39回目の繰り返し: 1.2772679619287193\n",
      "40回目の繰り返し: 1.3055459430641032\n",
      "41回目の繰り返し: 1.337003251041806\n",
      "42回目の繰り返し: 1.3718545416028995\n",
      "43回目の繰り返し: 1.4103450318892767\n",
      "44回目の繰り返し: 1.45275242012862\n",
      "45回目の繰り返し: 1.4993882648991232\n",
      "46回目の繰り返し: 1.5505981212355868\n",
      "47回目の繰り返し: 1.6067592190839344\n",
      "48回目の繰り返し: 1.668273603993058\n",
      "49回目の繰り返し: 1.7355532043860777\n",
      "50回目の繰り返し: 1.808990860818229\n",
      "51回目の繰り返し: 1.8889073598039459\n",
      "52回目の繰り返し: 1.9754581315085507\n",
      "53回目の繰り返し: 2.0684736064332734\n",
      "54回目の繰り返し: 2.1671941898224545\n",
      "55回目の繰り返し: 2.269847966276492\n",
      "56回目の繰り返し: 2.3730214847694446\n",
      "57回目の繰り返し: 2.470834026451166\n",
      "58回目の繰り返し: 2.5541360251515606\n",
      "59回目の繰り返し: 2.6104326089439747\n",
      "60回目の繰り返し: 2.6258913380254647\n",
      "61回目の繰り返し: 2.5906854364775715\n",
      "62回目の繰り返し: 2.506136825910509\n",
      "63回目の繰り返し: 2.387286480309533\n",
      "64回目の繰り返し: 2.255562448824859\n",
      "65回目の繰り返し: 2.127404358549408\n",
      "66回目の繰り返し: 2.009960064541425\n",
      "67回目の繰り返し: 1.9043764121601146\n",
      "68回目の繰り返し: 1.8097808396553112\n",
      "69回目の繰り返し: 1.7250160724592685\n",
      "70回目の繰り返し: 1.6490107066131365\n",
      "71回目の繰り返し: 1.5807951968909024\n",
      "72回目の繰り返し: 1.5194980579541033\n",
      "73回目の繰り返し: 1.4643560350608265\n",
      "74回目の繰り返し: 1.4147232270641994\n",
      "75回目の繰り返し: 1.370072624370299\n",
      "76回目の繰り返し: 1.3299916608868931\n",
      "77回目の繰り返し: 1.294175357281822\n",
      "78回目の繰り返し: 1.2624201022362538\n",
      "79回目の繰り返し: 1.234620171668675\n",
      "80回目の繰り返し: 1.2107684942442598\n",
      "81回目の繰り返し: 1.1909630351136908\n",
      "82回目の繰り返し: 1.175420471495386\n",
      "83回目の繰り返し: 1.164499577954534\n",
      "84回目の繰り返し: 1.1587380240167124\n",
      "85回目の繰り返し: 1.1589083231473227\n",
      "86回目の繰り返し: 1.1661017992731917\n",
      "87回目の繰り返し: 1.181854080635425\n",
      "88回目の繰り返し: 1.2083320212644932\n",
      "89回目の繰り返し: 1.2486089897400745\n",
      "90回目の繰り返し: 1.3070567843904153\n",
      "91回目の繰り返し: 1.3898537316311603\n",
      "92回目の繰り返し: 1.5054697089813545\n",
      "93回目の繰り返し: 1.6645047817628686\n",
      "94回目の繰り返し: 1.876862980356722\n",
      "95回目の繰り返し: 2.141247356924619\n",
      "96回目の繰り返し: 2.4201048899391306\n",
      "97回目の繰り返し: 2.611291114772405\n",
      "98回目の繰り返し: 2.5892805498328086\n",
      "99回目の繰り返し: 2.3600243741577955\n",
      "100回目の繰り返し: 2.09551510910153\n",
      "101回目の繰り返し: 1.9537447853115066\n",
      "102回目の繰り返し: 2.0097599136734807\n",
      "103回目の繰り返し: 2.26673803923795\n",
      "104回目の繰り返し: 2.5211343381756564\n",
      "105回目の繰り返し: 2.4544745236356498\n",
      "106回目の繰り返し: 2.190394348769711\n",
      "107回目の繰り返し: 1.9783677788219236\n",
      "108回目の繰り返し: 1.846465112560982\n",
      "109回目の繰り返し: 1.7652697065355607\n",
      "110回目の繰り返し: 1.7119187580964796\n",
      "111回目の繰り返し: 1.674874453862109\n",
      "112回目の繰り返し: 1.6479490264504184\n",
      "113回目の繰り返し: 1.627771541690331\n",
      "114回目の繰り返し: 1.6123041217288783\n",
      "115回目の繰り返し: 1.6002424310413117\n",
      "116回目の繰り返し: 1.5906777490039057\n",
      "117回目の繰り返し: 1.582933371019521\n",
      "118回目の繰り返し: 1.576469076158988\n",
      "119回目の繰り返し: 1.570828515439533\n",
      "120回目の繰り返し: 1.565608275055725\n",
      "121回目の繰り返し: 1.5604410256067685\n",
      "122回目の繰り返し: 1.5549875826855544\n",
      "123回目の繰り返し: 1.5489354170702054\n",
      "124回目の繰り返し: 1.542001915100535\n",
      "125回目の繰り返し: 1.533941129894911\n",
      "126回目の繰り返し: 1.5245527374633507\n",
      "127回目の繰り返し: 1.5136917310366595\n",
      "128回目の繰り返し: 1.5012771568901913\n",
      "129回目の繰り返し: 1.4872980899777697\n",
      "130回目の繰り返し: 1.471815209745167\n",
      "131回目の繰り返し: 1.454956864103581\n",
      "132回目の繰り返し: 1.4369093910603001\n",
      "133回目の繰り返し: 1.417902549350116\n",
      "134回目の繰り返し: 1.3981919111399603\n",
      "135回目の繰り返し: 1.3780406798550073\n",
      "136回目の繰り返し: 1.3577034023861574\n",
      "137回目の繰り返し: 1.337413447924488\n",
      "138回目の繰り返し: 1.3173751456244176\n",
      "139回目の繰り返し: 1.2977604507485305\n",
      "140回目の繰り返し: 1.2787092473372328\n",
      "141回目の繰り返し: 1.2603320451343833\n",
      "142回目の繰り返し: 1.2427138619412978\n",
      "143回目の繰り返し: 1.22591835833424\n",
      "144回目の繰り返し: 1.209991645181031\n",
      "145回目の繰り返し: 1.1949654957231342\n",
      "146回目の繰り返し: 1.1808599094150052\n",
      "147回目の繰り返し: 1.1676850935552308\n",
      "148回目の繰り返し: 1.155442975564245\n",
      "149回目の繰り返し: 1.1441283632726558\n",
      "150回目の繰り返し: 1.1337298548254608\n",
      "151回目の繰り返し: 1.124230577482195\n",
      "152回目の繰り返し: 1.115608812241906\n",
      "153回目の繰り返し: 1.1078385419320274\n",
      "154回目の繰り返し: 1.100889944775401\n",
      "155回目の繰り返し: 1.0947298441727291\n",
      "156回目の繰り返し: 1.0893221176521002\n",
      "157回目の繰り返し: 1.0846280636296093\n",
      "158回目の繰り返し: 1.0806067226328118\n",
      "159回目の繰り返し: 1.077215149555725\n",
      "160回目の繰り返し: 1.074408634443313\n",
      "161回目の繰り返し: 1.0721408708115423\n",
      "162回目の繰り返し: 1.0703640721548702\n",
      "163回目の繰り返し: 1.0690290389132355\n",
      "164回目の繰り返し: 1.0680851794585418\n",
      "165回目の繰り返し: 1.0674804899307597\n",
      "166回目の繰り返し: 1.0671614985820752\n",
      "167回目の繰り返し: 1.0670731812283012\n",
      "168回目の繰り返し: 1.06715885508591\n",
      "169回目の繰り返し: 1.067360059042717\n",
      "170回目の繰り返し: 1.067616429284053\n",
      "171回目の繰り返し: 1.067865580111982\n",
      "172回目の繰り返し: 1.0680430008607653\n",
      "173回目の繰り返し: 1.0680819810954438\n",
      "174回目の繰り返し: 1.0679135776042359\n",
      "175回目の繰り返し: 1.0674666381514406\n",
      "176回目の繰り返し: 1.0666678983640725\n",
      "177回目の繰り返し: 1.0654421694168206\n",
      "178回目の繰り返し: 1.06371263516982\n",
      "179回目の繰り返し: 1.0614012777422466\n",
      "180回目の繰り返し: 1.0584294501492053\n",
      "181回目の繰り返し: 1.0547186129739885\n",
      "182回目の繰り返し: 1.0501912489169312\n",
      "183回目の繰り返し: 1.0447719639843571\n",
      "184回目の繰り返し: 1.038388776794104\n",
      "185回目の繰り返し: 1.0309745877383878\n",
      "186回目の繰り返し: 1.0224688073936739\n",
      "187回目の繰り返し: 1.0128191091069119\n",
      "188回目の繰り返し: 1.0019832543560176\n",
      "189回目の繰り返し: 0.9899309226265202\n",
      "190回目の繰り返し: 0.9766454614103472\n",
      "191回目の繰り返し: 0.962125458606373\n",
      "192回目の繰り返し: 0.9463860311474116\n",
      "193回目の繰り返し: 0.9294597224131055\n",
      "194回目の繰り返し: 0.9113969088007378\n",
      "195回目の繰り返し: 0.8922656338579725\n",
      "196回目の繰り返し: 0.8721508167650108\n",
      "197回目の繰り返し: 0.8511528193543852\n",
      "198回目の繰り返し: 0.8293853991636145\n",
      "199回目の繰り返し: 0.8069731214743111\n",
      "200回目の繰り返し: 0.7840483454023371\n",
      "201回目の繰り返し: 0.7607479331915294\n",
      "202回目の繰り返し: 0.7372098533491469\n",
      "203回目の繰り返し: 0.7135698545867265\n",
      "204回目の繰り返し: 0.6899583777956612\n",
      "205回目の繰り返し: 0.666497849215474\n",
      "206回目の繰り返し: 0.6433004627084357\n",
      "207回目の繰り返し: 0.6204665173926099\n",
      "208回目の繰り返し: 0.5980833338352861\n",
      "209回目の繰り返し: 0.576224732149609\n",
      "210回目の繰り返し: 0.5549510223681543\n",
      "211回目の繰り返し: 0.5343094334869638\n",
      "212回目の繰り返し: 0.5143348933977362\n",
      "213回目の繰り返し: 0.4950510669190002\n",
      "214回目の繰り返し: 0.47647156198489454\n",
      "215回目の繰り返し: 0.45860122258661923\n",
      "216回目の繰り返し: 0.4414374393997931\n",
      "217回目の繰り返し: 0.4249714231362999\n",
      "218回目の繰り返し: 0.40918939987345915\n",
      "219回目の繰り返し: 0.3940737009762608\n",
      "220回目の繰り返し: 0.3796037317689136\n",
      "221回目の繰り返し: 0.36575681262680776\n",
      "222回目の繰り返し: 0.3525088934788517\n",
      "223回目の繰り返し: 0.3398351479875783\n",
      "224回目の繰り返し: 0.32771045717489106\n",
      "225回目の繰り返し: 0.3161097942369434\n",
      "226回目の繰り返し: 0.30500852314807414\n",
      "227回目の繰り返し: 0.29438262361104706\n",
      "228回目の繰り返し: 0.2842088543082553\n",
      "229回目の繰り返し: 0.27446486539141\n",
      "230回目の繰り返し: 0.2651292699716477\n",
      "231回目の繰り返し: 0.2561816830929465\n",
      "232回目の繰り返し: 0.2476027353999608\n",
      "233回目の繰り返し: 0.23937406755542887\n",
      "234回目の繰り返し: 0.23147831037696917\n",
      "235回目の繰り返し: 0.22389905471258528\n",
      "236回目の繰り返し: 0.21662081427242433\n",
      "237回目の繰り返し: 0.20962898393964918\n",
      "238回目の繰り返し: 0.20290979550827168\n",
      "239回目の繰り返し: 0.19645027233430717\n",
      "240回目の繰り返し: 0.19023818399656303\n",
      "241回目の繰り返し: 0.1842620017815538\n",
      "242回目の繰り返し: 0.17851085554132048\n",
      "243回目の繰り返し: 0.17297449232003212\n",
      "244回目の繰り返し: 0.16764323697648903\n",
      "245回目の繰り返し: 0.16250795494489595\n",
      "246回目の繰り返し: 0.15756001717573515\n",
      "247回目の繰り返し: 0.15279126726372552\n",
      "248回目の繰り返し: 0.14819399070754494\n",
      "249回目の繰り返し: 0.14376088623123132\n",
      "250回目の繰り返し: 0.13948503907638202\n",
      "251回目の繰り返し: 0.13535989615568175\n",
      "252回目の繰り返し: 0.13137924296557615\n",
      "253回目の繰り返し: 0.1275371821391808\n",
      "254回目の繰り返し: 0.12382811353806808\n",
      "255回目の繰り返し: 0.12024671576741677\n",
      "256回目の繰り返し: 0.11678792901776372\n",
      "257回目の繰り返し: 0.11344693913421987\n",
      "258回目の繰り返し: 0.11021916281954751\n",
      "259回目の繰り返し: 0.10710023388841743\n",
      "260回目の繰り返し: 0.10408599049024354\n",
      "261回目の繰り返し: 0.10117246322555142\n",
      "262回目の繰り返し: 0.09835586409106624\n",
      "263回目の繰り返し: 0.09563257618271993\n",
      "264回目の繰り返し: 0.09299914410804029\n",
      "265回目の繰り返し: 0.09045226504054295\n",
      "266回目の繰り返し: 0.08798878038043713\n",
      "267回目の繰り返し: 0.08560566796459071\n",
      "268回目の繰り返し: 0.08330003478960035\n",
      "269回目の繰り返し: 0.08106911020846262\n",
      "270回目の繰り返し: 0.07891023956371167\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "271回目の繰り返し: 0.07682087822606846\n",
      "272回目の繰り返し: 0.07479858600763571\n",
      "273回目の繰り返し: 0.07284102192296416\n",
      "274回目の繰り返し: 0.07094593927129311\n",
      "275回目の繰り返し: 0.06911118101915194\n",
      "276回目の繰り返し: 0.06733467545875722\n",
      "277回目の繰り返し: 0.06561443212528616\n",
      "278回目の繰り返し: 0.06394853795319445\n",
      "279回目の繰り返し: 0.0623351536559069\n",
      "280回目の繰り返し: 0.0607725103127657\n",
      "281回目の繰り返し: 0.05925890614986541\n",
      "282回目の繰り返し: 0.05779270349986104\n",
      "283回目の繰り返し: 0.05637232593262126\n",
      "284回目の繰り返し: 0.054996255539852205\n",
      "285回目の繰り返し: 0.05366303036846474\n",
      "286回目の繰り返し: 0.05237124199204993\n",
      "287回目の繰り返し: 0.051119533207019514\n",
      "288回目の繰り返し: 0.0499065958537849\n",
      "289回目の繰り返し: 0.0487311687471473\n",
      "290回目の繰り返し: 0.047592035714203326\n",
      "291回目の繰り返し: 0.046488023730174854\n",
      "292回目の繰り返し: 0.04541800114824676\n",
      "293回目の繰り返し: 0.04438087601731754\n",
      "294回目の繰り返し: 0.04337559448010011\n",
      "295回目の繰り返し: 0.04240113925120284\n",
      "296回目の繰り返し: 0.0414565281660409\n",
      "297回目の繰り返し: 0.04054081279980492\n",
      "298回目の繰り返し: 0.03965307715005564\n",
      "299回目の繰り返し: 0.03879243638179578\n",
      "300回目の繰り返し: 0.03795803562875125\n",
      "301回目の繰り返し: 0.03714904885065966\n",
      "302回目の繰り返し: 0.03636467774058057\n",
      "303回目の繰り返し: 0.03560415068205771\n",
      "304回目の繰り返し: 0.034866721752181425\n",
      "305回目の繰り返し: 0.03415166976737129\n",
      "306回目の繰り返し: 0.033458297372517334\n",
      "307回目の繰り返し: 0.032785930166904106\n",
      "308回目の繰り返し: 0.03213391586939592\n",
      "309回目の繰り返し: 0.031501623517814575\n",
      "310回目の繰り返し: 0.030888442702409678\n",
      "311回目の繰り返し: 0.030293782830512393\n",
      "312回目の繰り返し: 0.02971707242264249\n",
      "313回目の繰り返し: 0.02915775843603712\n",
      "314回目の繰り返し: 0.028615305616531434\n",
      "315回目の繰り返し: 0.02808919587664555\n",
      "316回目の繰り返し: 0.027578927697754718\n",
      "317回目の繰り返し: 0.027084015556464017\n",
      "318回目の繰り返し: 0.026603989373720566\n",
      "319回目の繰り返し: 0.026138393985196982\n",
      "320回目の繰り返し: 0.025686788632146473\n",
      "321回目の繰り返し: 0.02524874647215427\n",
      "322回目の繰り返し: 0.024823854108434747\n",
      "323回目の繰り返し: 0.024411711137451303\n",
      "324回目の繰り返し: 0.02401192971263582\n",
      "325回目の繰り返し: 0.02362413412592705\n",
      "326回目の繰り返し: 0.023247960403620548\n",
      "327回目の繰り返し: 0.022883055917928316\n",
      "328回目の繰り返し: 0.022529079012815645\n",
      "329回目の繰り返し: 0.022185698642974466\n",
      "330回目の繰り返し: 0.02185259402736229\n",
      "331回目の繰り返し: 0.02152945431297749\n",
      "332回目の繰り返し: 0.021215978253259188\n",
      "333回目の繰り返し: 0.02091187389618563\n",
      "334回目の繰り返し: 0.02061685828419278\n",
      "335回目の繰り返し: 0.020330657165152255\n",
      "336回目の繰り返し: 0.020053004712393276\n",
      "337回目の繰り返し: 0.01978364325530816\n"
     ]
    },
    {
     "data": {
      "application/javascript": [
       "/* Put everything inside the global mpl namespace */\n",
       "window.mpl = {};\n",
       "\n",
       "\n",
       "mpl.get_websocket_type = function() {\n",
       "    if (typeof(WebSocket) !== 'undefined') {\n",
       "        return WebSocket;\n",
       "    } else if (typeof(MozWebSocket) !== 'undefined') {\n",
       "        return MozWebSocket;\n",
       "    } else {\n",
       "        alert('Your browser does not have WebSocket support. ' +\n",
       "              'Please try Chrome, Safari or Firefox ≥ 6. ' +\n",
       "              'Firefox 4 and 5 are also supported but you ' +\n",
       "              'have to enable WebSockets in about:config.');\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure = function(figure_id, websocket, ondownload, parent_element) {\n",
       "    this.id = figure_id;\n",
       "\n",
       "    this.ws = websocket;\n",
       "\n",
       "    this.supports_binary = (this.ws.binaryType != undefined);\n",
       "\n",
       "    if (!this.supports_binary) {\n",
       "        var warnings = document.getElementById(\"mpl-warnings\");\n",
       "        if (warnings) {\n",
       "            warnings.style.display = 'block';\n",
       "            warnings.textContent = (\n",
       "                \"This browser does not support binary websocket messages. \" +\n",
       "                    \"Performance may be slow.\");\n",
       "        }\n",
       "    }\n",
       "\n",
       "    this.imageObj = new Image();\n",
       "\n",
       "    this.context = undefined;\n",
       "    this.message = undefined;\n",
       "    this.canvas = undefined;\n",
       "    this.rubberband_canvas = undefined;\n",
       "    this.rubberband_context = undefined;\n",
       "    this.format_dropdown = undefined;\n",
       "\n",
       "    this.image_mode = 'full';\n",
       "\n",
       "    this.root = $('<div/>');\n",
       "    this._root_extra_style(this.root)\n",
       "    this.root.attr('style', 'display: inline-block');\n",
       "\n",
       "    $(parent_element).append(this.root);\n",
       "\n",
       "    this._init_header(this);\n",
       "    this._init_canvas(this);\n",
       "    this._init_toolbar(this);\n",
       "\n",
       "    var fig = this;\n",
       "\n",
       "    this.waiting = false;\n",
       "\n",
       "    this.ws.onopen =  function () {\n",
       "            fig.send_message(\"supports_binary\", {value: fig.supports_binary});\n",
       "            fig.send_message(\"send_image_mode\", {});\n",
       "            if (mpl.ratio != 1) {\n",
       "                fig.send_message(\"set_dpi_ratio\", {'dpi_ratio': mpl.ratio});\n",
       "            }\n",
       "            fig.send_message(\"refresh\", {});\n",
       "        }\n",
       "\n",
       "    this.imageObj.onload = function() {\n",
       "            if (fig.image_mode == 'full') {\n",
       "                // Full images could contain transparency (where diff images\n",
       "                // almost always do), so we need to clear the canvas so that\n",
       "                // there is no ghosting.\n",
       "                fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n",
       "            }\n",
       "            fig.context.drawImage(fig.imageObj, 0, 0);\n",
       "        };\n",
       "\n",
       "    this.imageObj.onunload = function() {\n",
       "        fig.ws.close();\n",
       "    }\n",
       "\n",
       "    this.ws.onmessage = this._make_on_message_function(this);\n",
       "\n",
       "    this.ondownload = ondownload;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_header = function() {\n",
       "    var titlebar = $(\n",
       "        '<div class=\"ui-dialog-titlebar ui-widget-header ui-corner-all ' +\n",
       "        'ui-helper-clearfix\"/>');\n",
       "    var titletext = $(\n",
       "        '<div class=\"ui-dialog-title\" style=\"width: 100%; ' +\n",
       "        'text-align: center; padding: 3px;\"/>');\n",
       "    titlebar.append(titletext)\n",
       "    this.root.append(titlebar);\n",
       "    this.header = titletext[0];\n",
       "}\n",
       "\n",
       "\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_canvas = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var canvas_div = $('<div/>');\n",
       "\n",
       "    canvas_div.attr('style', 'position: relative; clear: both; outline: 0');\n",
       "\n",
       "    function canvas_keyboard_event(event) {\n",
       "        return fig.key_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    canvas_div.keydown('key_press', canvas_keyboard_event);\n",
       "    canvas_div.keyup('key_release', canvas_keyboard_event);\n",
       "    this.canvas_div = canvas_div\n",
       "    this._canvas_extra_style(canvas_div)\n",
       "    this.root.append(canvas_div);\n",
       "\n",
       "    var canvas = $('<canvas/>');\n",
       "    canvas.addClass('mpl-canvas');\n",
       "    canvas.attr('style', \"left: 0; top: 0; z-index: 0; outline: 0\")\n",
       "\n",
       "    this.canvas = canvas[0];\n",
       "    this.context = canvas[0].getContext(\"2d\");\n",
       "\n",
       "    var backingStore = this.context.backingStorePixelRatio ||\n",
       "\tthis.context.webkitBackingStorePixelRatio ||\n",
       "\tthis.context.mozBackingStorePixelRatio ||\n",
       "\tthis.context.msBackingStorePixelRatio ||\n",
       "\tthis.context.oBackingStorePixelRatio ||\n",
       "\tthis.context.backingStorePixelRatio || 1;\n",
       "\n",
       "    mpl.ratio = (window.devicePixelRatio || 1) / backingStore;\n",
       "\n",
       "    var rubberband = $('<canvas/>');\n",
       "    rubberband.attr('style', \"position: absolute; left: 0; top: 0; z-index: 1;\")\n",
       "\n",
       "    var pass_mouse_events = true;\n",
       "\n",
       "    canvas_div.resizable({\n",
       "        start: function(event, ui) {\n",
       "            pass_mouse_events = false;\n",
       "        },\n",
       "        resize: function(event, ui) {\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "        stop: function(event, ui) {\n",
       "            pass_mouse_events = true;\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "    });\n",
       "\n",
       "    function mouse_event_fn(event) {\n",
       "        if (pass_mouse_events)\n",
       "            return fig.mouse_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    rubberband.mousedown('button_press', mouse_event_fn);\n",
       "    rubberband.mouseup('button_release', mouse_event_fn);\n",
       "    // Throttle sequential mouse events to 1 every 20ms.\n",
       "    rubberband.mousemove('motion_notify', mouse_event_fn);\n",
       "\n",
       "    rubberband.mouseenter('figure_enter', mouse_event_fn);\n",
       "    rubberband.mouseleave('figure_leave', mouse_event_fn);\n",
       "\n",
       "    canvas_div.on(\"wheel\", function (event) {\n",
       "        event = event.originalEvent;\n",
       "        event['data'] = 'scroll'\n",
       "        if (event.deltaY < 0) {\n",
       "            event.step = 1;\n",
       "        } else {\n",
       "            event.step = -1;\n",
       "        }\n",
       "        mouse_event_fn(event);\n",
       "    });\n",
       "\n",
       "    canvas_div.append(canvas);\n",
       "    canvas_div.append(rubberband);\n",
       "\n",
       "    this.rubberband = rubberband;\n",
       "    this.rubberband_canvas = rubberband[0];\n",
       "    this.rubberband_context = rubberband[0].getContext(\"2d\");\n",
       "    this.rubberband_context.strokeStyle = \"#000000\";\n",
       "\n",
       "    this._resize_canvas = function(width, height) {\n",
       "        // Keep the size of the canvas, canvas container, and rubber band\n",
       "        // canvas in synch.\n",
       "        canvas_div.css('width', width)\n",
       "        canvas_div.css('height', height)\n",
       "\n",
       "        canvas.attr('width', width * mpl.ratio);\n",
       "        canvas.attr('height', height * mpl.ratio);\n",
       "        canvas.attr('style', 'width: ' + width + 'px; height: ' + height + 'px;');\n",
       "\n",
       "        rubberband.attr('width', width);\n",
       "        rubberband.attr('height', height);\n",
       "    }\n",
       "\n",
       "    // Set the figure to an initial 600x600px, this will subsequently be updated\n",
       "    // upon first draw.\n",
       "    this._resize_canvas(600, 600);\n",
       "\n",
       "    // Disable right mouse context menu.\n",
       "    $(this.rubberband_canvas).bind(\"contextmenu\",function(e){\n",
       "        return false;\n",
       "    });\n",
       "\n",
       "    function set_focus () {\n",
       "        canvas.focus();\n",
       "        canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    window.setTimeout(set_focus, 100);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>');\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items) {\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) {\n",
       "            // put a spacer in here.\n",
       "            continue;\n",
       "        }\n",
       "        var button = $('<button/>');\n",
       "        button.addClass('ui-button ui-widget ui-state-default ui-corner-all ' +\n",
       "                        'ui-button-icon-only');\n",
       "        button.attr('role', 'button');\n",
       "        button.attr('aria-disabled', 'false');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "\n",
       "        var icon_img = $('<span/>');\n",
       "        icon_img.addClass('ui-button-icon-primary ui-icon');\n",
       "        icon_img.addClass(image);\n",
       "        icon_img.addClass('ui-corner-all');\n",
       "\n",
       "        var tooltip_span = $('<span/>');\n",
       "        tooltip_span.addClass('ui-button-text');\n",
       "        tooltip_span.html(tooltip);\n",
       "\n",
       "        button.append(icon_img);\n",
       "        button.append(tooltip_span);\n",
       "\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    var fmt_picker_span = $('<span/>');\n",
       "\n",
       "    var fmt_picker = $('<select/>');\n",
       "    fmt_picker.addClass('mpl-toolbar-option ui-widget ui-widget-content');\n",
       "    fmt_picker_span.append(fmt_picker);\n",
       "    nav_element.append(fmt_picker_span);\n",
       "    this.format_dropdown = fmt_picker[0];\n",
       "\n",
       "    for (var ind in mpl.extensions) {\n",
       "        var fmt = mpl.extensions[ind];\n",
       "        var option = $(\n",
       "            '<option/>', {selected: fmt === mpl.default_extension}).html(fmt);\n",
       "        fmt_picker.append(option);\n",
       "    }\n",
       "\n",
       "    // Add hover states to the ui-buttons\n",
       "    $( \".ui-button\" ).hover(\n",
       "        function() { $(this).addClass(\"ui-state-hover\");},\n",
       "        function() { $(this).removeClass(\"ui-state-hover\");}\n",
       "    );\n",
       "\n",
       "    var status_bar = $('<span class=\"mpl-message\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.request_resize = function(x_pixels, y_pixels) {\n",
       "    // Request matplotlib to resize the figure. Matplotlib will then trigger a resize in the client,\n",
       "    // which will in turn request a refresh of the image.\n",
       "    this.send_message('resize', {'width': x_pixels, 'height': y_pixels});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_message = function(type, properties) {\n",
       "    properties['type'] = type;\n",
       "    properties['figure_id'] = this.id;\n",
       "    this.ws.send(JSON.stringify(properties));\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_draw_message = function() {\n",
       "    if (!this.waiting) {\n",
       "        this.waiting = true;\n",
       "        this.ws.send(JSON.stringify({type: \"draw\", figure_id: this.id}));\n",
       "    }\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    var format_dropdown = fig.format_dropdown;\n",
       "    var format = format_dropdown.options[format_dropdown.selectedIndex].value;\n",
       "    fig.ondownload(fig, format);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_resize = function(fig, msg) {\n",
       "    var size = msg['size'];\n",
       "    if (size[0] != fig.canvas.width || size[1] != fig.canvas.height) {\n",
       "        fig._resize_canvas(size[0], size[1]);\n",
       "        fig.send_message(\"refresh\", {});\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_rubberband = function(fig, msg) {\n",
       "    var x0 = msg['x0'] / mpl.ratio;\n",
       "    var y0 = (fig.canvas.height - msg['y0']) / mpl.ratio;\n",
       "    var x1 = msg['x1'] / mpl.ratio;\n",
       "    var y1 = (fig.canvas.height - msg['y1']) / mpl.ratio;\n",
       "    x0 = Math.floor(x0) + 0.5;\n",
       "    y0 = Math.floor(y0) + 0.5;\n",
       "    x1 = Math.floor(x1) + 0.5;\n",
       "    y1 = Math.floor(y1) + 0.5;\n",
       "    var min_x = Math.min(x0, x1);\n",
       "    var min_y = Math.min(y0, y1);\n",
       "    var width = Math.abs(x1 - x0);\n",
       "    var height = Math.abs(y1 - y0);\n",
       "\n",
       "    fig.rubberband_context.clearRect(\n",
       "        0, 0, fig.canvas.width / mpl.ratio, fig.canvas.height / mpl.ratio);\n",
       "\n",
       "    fig.rubberband_context.strokeRect(min_x, min_y, width, height);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_figure_label = function(fig, msg) {\n",
       "    // Updates the figure title.\n",
       "    fig.header.textContent = msg['label'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_cursor = function(fig, msg) {\n",
       "    var cursor = msg['cursor'];\n",
       "    switch(cursor)\n",
       "    {\n",
       "    case 0:\n",
       "        cursor = 'pointer';\n",
       "        break;\n",
       "    case 1:\n",
       "        cursor = 'default';\n",
       "        break;\n",
       "    case 2:\n",
       "        cursor = 'crosshair';\n",
       "        break;\n",
       "    case 3:\n",
       "        cursor = 'move';\n",
       "        break;\n",
       "    }\n",
       "    fig.rubberband_canvas.style.cursor = cursor;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_message = function(fig, msg) {\n",
       "    fig.message.textContent = msg['message'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_draw = function(fig, msg) {\n",
       "    // Request the server to send over a new figure.\n",
       "    fig.send_draw_message();\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_image_mode = function(fig, msg) {\n",
       "    fig.image_mode = msg['mode'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Called whenever the canvas gets updated.\n",
       "    this.send_message(\"ack\", {});\n",
       "}\n",
       "\n",
       "// A function to construct a web socket function for onmessage handling.\n",
       "// Called in the figure constructor.\n",
       "mpl.figure.prototype._make_on_message_function = function(fig) {\n",
       "    return function socket_on_message(evt) {\n",
       "        if (evt.data instanceof Blob) {\n",
       "            /* FIXME: We get \"Resource interpreted as Image but\n",
       "             * transferred with MIME type text/plain:\" errors on\n",
       "             * Chrome.  But how to set the MIME type?  It doesn't seem\n",
       "             * to be part of the websocket stream */\n",
       "            evt.data.type = \"image/png\";\n",
       "\n",
       "            /* Free the memory for the previous frames */\n",
       "            if (fig.imageObj.src) {\n",
       "                (window.URL || window.webkitURL).revokeObjectURL(\n",
       "                    fig.imageObj.src);\n",
       "            }\n",
       "\n",
       "            fig.imageObj.src = (window.URL || window.webkitURL).createObjectURL(\n",
       "                evt.data);\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "        else if (typeof evt.data === 'string' && evt.data.slice(0, 21) == \"data:image/png;base64\") {\n",
       "            fig.imageObj.src = evt.data;\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        var msg = JSON.parse(evt.data);\n",
       "        var msg_type = msg['type'];\n",
       "\n",
       "        // Call the  \"handle_{type}\" callback, which takes\n",
       "        // the figure and JSON message as its only arguments.\n",
       "        try {\n",
       "            var callback = fig[\"handle_\" + msg_type];\n",
       "        } catch (e) {\n",
       "            console.log(\"No handler for the '\" + msg_type + \"' message type: \", msg);\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        if (callback) {\n",
       "            try {\n",
       "                // console.log(\"Handling '\" + msg_type + \"' message: \", msg);\n",
       "                callback(fig, msg);\n",
       "            } catch (e) {\n",
       "                console.log(\"Exception inside the 'handler_\" + msg_type + \"' callback:\", e, e.stack, msg);\n",
       "            }\n",
       "        }\n",
       "    };\n",
       "}\n",
       "\n",
       "// from http://stackoverflow.com/questions/1114465/getting-mouse-location-in-canvas\n",
       "mpl.findpos = function(e) {\n",
       "    //this section is from http://www.quirksmode.org/js/events_properties.html\n",
       "    var targ;\n",
       "    if (!e)\n",
       "        e = window.event;\n",
       "    if (e.target)\n",
       "        targ = e.target;\n",
       "    else if (e.srcElement)\n",
       "        targ = e.srcElement;\n",
       "    if (targ.nodeType == 3) // defeat Safari bug\n",
       "        targ = targ.parentNode;\n",
       "\n",
       "    // jQuery normalizes the pageX and pageY\n",
       "    // pageX,Y are the mouse positions relative to the document\n",
       "    // offset() returns the position of the element relative to the document\n",
       "    var x = e.pageX - $(targ).offset().left;\n",
       "    var y = e.pageY - $(targ).offset().top;\n",
       "\n",
       "    return {\"x\": x, \"y\": y};\n",
       "};\n",
       "\n",
       "/*\n",
       " * return a copy of an object with only non-object keys\n",
       " * we need this to avoid circular references\n",
       " * http://stackoverflow.com/a/24161582/3208463\n",
       " */\n",
       "function simpleKeys (original) {\n",
       "  return Object.keys(original).reduce(function (obj, key) {\n",
       "    if (typeof original[key] !== 'object')\n",
       "        obj[key] = original[key]\n",
       "    return obj;\n",
       "  }, {});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.mouse_event = function(event, name) {\n",
       "    var canvas_pos = mpl.findpos(event)\n",
       "\n",
       "    if (name === 'button_press')\n",
       "    {\n",
       "        this.canvas.focus();\n",
       "        this.canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    var x = canvas_pos.x * mpl.ratio;\n",
       "    var y = canvas_pos.y * mpl.ratio;\n",
       "\n",
       "    this.send_message(name, {x: x, y: y, button: event.button,\n",
       "                             step: event.step,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "\n",
       "    /* This prevents the web browser from automatically changing to\n",
       "     * the text insertion cursor when the button is pressed.  We want\n",
       "     * to control all of the cursor setting manually through the\n",
       "     * 'cursor' event from matplotlib */\n",
       "    event.preventDefault();\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    // Handle any extra behaviour associated with a key event\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.key_event = function(event, name) {\n",
       "\n",
       "    // Prevent repeat events\n",
       "    if (name == 'key_press')\n",
       "    {\n",
       "        if (event.which === this._key)\n",
       "            return;\n",
       "        else\n",
       "            this._key = event.which;\n",
       "    }\n",
       "    if (name == 'key_release')\n",
       "        this._key = null;\n",
       "\n",
       "    var value = '';\n",
       "    if (event.ctrlKey && event.which != 17)\n",
       "        value += \"ctrl+\";\n",
       "    if (event.altKey && event.which != 18)\n",
       "        value += \"alt+\";\n",
       "    if (event.shiftKey && event.which != 16)\n",
       "        value += \"shift+\";\n",
       "\n",
       "    value += 'k';\n",
       "    value += event.which.toString();\n",
       "\n",
       "    this._key_event_extra(event, name);\n",
       "\n",
       "    this.send_message(name, {key: value,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onclick = function(name) {\n",
       "    if (name == 'download') {\n",
       "        this.handle_save(this, null);\n",
       "    } else {\n",
       "        this.send_message(\"toolbar_button\", {name: name});\n",
       "    }\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onmouseover = function(tooltip) {\n",
       "    this.message.textContent = tooltip;\n",
       "};\n",
       "mpl.toolbar_items = [[\"Home\", \"Reset original view\", \"fa fa-home icon-home\", \"home\"], [\"Back\", \"Back to previous view\", \"fa fa-arrow-left icon-arrow-left\", \"back\"], [\"Forward\", \"Forward to next view\", \"fa fa-arrow-right icon-arrow-right\", \"forward\"], [\"\", \"\", \"\", \"\"], [\"Pan\", \"Pan axes with left mouse, zoom with right\", \"fa fa-arrows icon-move\", \"pan\"], [\"Zoom\", \"Zoom to rectangle\", \"fa fa-square-o icon-check-empty\", \"zoom\"], [\"\", \"\", \"\", \"\"], [\"Download\", \"Download plot\", \"fa fa-floppy-o icon-save\", \"download\"]];\n",
       "\n",
       "mpl.extensions = [\"eps\", \"jpeg\", \"pdf\", \"png\", \"ps\", \"raw\", \"svg\", \"tif\"];\n",
       "\n",
       "mpl.default_extension = \"png\";var comm_websocket_adapter = function(comm) {\n",
       "    // Create a \"websocket\"-like object which calls the given IPython comm\n",
       "    // object with the appropriate methods. Currently this is a non binary\n",
       "    // socket, so there is still some room for performance tuning.\n",
       "    var ws = {};\n",
       "\n",
       "    ws.close = function() {\n",
       "        comm.close()\n",
       "    };\n",
       "    ws.send = function(m) {\n",
       "        //console.log('sending', m);\n",
       "        comm.send(m);\n",
       "    };\n",
       "    // Register the callback with on_msg.\n",
       "    comm.on_msg(function(msg) {\n",
       "        //console.log('receiving', msg['content']['data'], msg);\n",
       "        // Pass the mpl event to the overridden (by mpl) onmessage function.\n",
       "        ws.onmessage(msg['content']['data'])\n",
       "    });\n",
       "    return ws;\n",
       "}\n",
       "\n",
       "mpl.mpl_figure_comm = function(comm, msg) {\n",
       "    // This is the function which gets called when the mpl process\n",
       "    // starts-up an IPython Comm through the \"matplotlib\" channel.\n",
       "\n",
       "    var id = msg.content.data.id;\n",
       "    // Get hold of the div created by the display call when the Comm\n",
       "    // socket was opened in Python.\n",
       "    var element = $(\"#\" + id);\n",
       "    var ws_proxy = comm_websocket_adapter(comm)\n",
       "\n",
       "    function ondownload(figure, format) {\n",
       "        window.open(figure.imageObj.src);\n",
       "    }\n",
       "\n",
       "    var fig = new mpl.figure(id, ws_proxy,\n",
       "                           ondownload,\n",
       "                           element.get(0));\n",
       "\n",
       "    // Call onopen now - mpl needs it, as it is assuming we've passed it a real\n",
       "    // web socket which is closed, not our websocket->open comm proxy.\n",
       "    ws_proxy.onopen();\n",
       "\n",
       "    fig.parent_element = element.get(0);\n",
       "    fig.cell_info = mpl.find_output_cell(\"<div id='\" + id + \"'></div>\");\n",
       "    if (!fig.cell_info) {\n",
       "        console.error(\"Failed to find cell for figure\", id, fig);\n",
       "        return;\n",
       "    }\n",
       "\n",
       "    var output_index = fig.cell_info[2]\n",
       "    var cell = fig.cell_info[0];\n",
       "\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.handle_close = function(fig, msg) {\n",
       "    var width = fig.canvas.width/mpl.ratio\n",
       "    fig.root.unbind('remove')\n",
       "\n",
       "    // Update the output cell to use the data from the current canvas.\n",
       "    fig.push_to_output();\n",
       "    var dataURL = fig.canvas.toDataURL();\n",
       "    // Re-enable the keyboard manager in IPython - without this line, in FF,\n",
       "    // the notebook keyboard shortcuts fail.\n",
       "    IPython.keyboard_manager.enable()\n",
       "    $(fig.parent_element).html('<img src=\"' + dataURL + '\" width=\"' + width + '\">');\n",
       "    fig.close_ws(fig, msg);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.close_ws = function(fig, msg){\n",
       "    fig.send_message('closing', msg);\n",
       "    // fig.ws.close()\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.push_to_output = function(remove_interactive) {\n",
       "    // Turn the data on the canvas into data in the output cell.\n",
       "    var width = this.canvas.width/mpl.ratio\n",
       "    var dataURL = this.canvas.toDataURL();\n",
       "    this.cell_info[1]['text/html'] = '<img src=\"' + dataURL + '\" width=\"' + width + '\">';\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Tell IPython that the notebook contents must change.\n",
       "    IPython.notebook.set_dirty(true);\n",
       "    this.send_message(\"ack\", {});\n",
       "    var fig = this;\n",
       "    // Wait a second, then push the new image to the DOM so\n",
       "    // that it is saved nicely (might be nice to debounce this).\n",
       "    setTimeout(function () { fig.push_to_output() }, 1000);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>');\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items){\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) { continue; };\n",
       "\n",
       "        var button = $('<button class=\"btn btn-default\" href=\"#\" title=\"' + name + '\"><i class=\"fa ' + image + ' fa-lg\"></i></button>');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    // Add the status bar.\n",
       "    var status_bar = $('<span class=\"mpl-message\" style=\"text-align:right; float: right;\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "\n",
       "    // Add the close button to the window.\n",
       "    var buttongrp = $('<div class=\"btn-group inline pull-right\"></div>');\n",
       "    var button = $('<button class=\"btn btn-mini btn-primary\" href=\"#\" title=\"Stop Interaction\"><i class=\"fa fa-power-off icon-remove icon-large\"></i></button>');\n",
       "    button.click(function (evt) { fig.handle_close(fig, {}); } );\n",
       "    button.mouseover('Stop Interaction', toolbar_mouse_event);\n",
       "    buttongrp.append(button);\n",
       "    var titlebar = this.root.find($('.ui-dialog-titlebar'));\n",
       "    titlebar.prepend(buttongrp);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(el){\n",
       "    var fig = this\n",
       "    el.on(\"remove\", function(){\n",
       "\tfig.close_ws(fig, {});\n",
       "    });\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(el){\n",
       "    // this is important to make the div 'focusable\n",
       "    el.attr('tabindex', 0)\n",
       "    // reach out to IPython and tell the keyboard manager to turn it's self\n",
       "    // off when our div gets focus\n",
       "\n",
       "    // location in version 3\n",
       "    if (IPython.notebook.keyboard_manager) {\n",
       "        IPython.notebook.keyboard_manager.register_events(el);\n",
       "    }\n",
       "    else {\n",
       "        // location in version 2\n",
       "        IPython.keyboard_manager.register_events(el);\n",
       "    }\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    var manager = IPython.notebook.keyboard_manager;\n",
       "    if (!manager)\n",
       "        manager = IPython.keyboard_manager;\n",
       "\n",
       "    // Check for shift+enter\n",
       "    if (event.shiftKey && event.which == 13) {\n",
       "        this.canvas_div.blur();\n",
       "        // select the cell after this one\n",
       "        var index = IPython.notebook.find_cell_index(this.cell_info[0]);\n",
       "        IPython.notebook.select(index + 1);\n",
       "    }\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    fig.ondownload(fig, null);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.find_output_cell = function(html_output) {\n",
       "    // Return the cell and output element which can be found *uniquely* in the notebook.\n",
       "    // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n",
       "    // IPython event is triggered only after the cells have been serialised, which for\n",
       "    // our purposes (turning an active figure into a static one), is too late.\n",
       "    var cells = IPython.notebook.get_cells();\n",
       "    var ncells = cells.length;\n",
       "    for (var i=0; i<ncells; i++) {\n",
       "        var cell = cells[i];\n",
       "        if (cell.cell_type === 'code'){\n",
       "            for (var j=0; j<cell.output_area.outputs.length; j++) {\n",
       "                var data = cell.output_area.outputs[j];\n",
       "                if (data.data) {\n",
       "                    // IPython >= 3 moved mimebundle to data attribute of output\n",
       "                    data = data.data;\n",
       "                }\n",
       "                if (data['text/html'] == html_output) {\n",
       "                    return [cell, data, j];\n",
       "                }\n",
       "            }\n",
       "        }\n",
       "    }\n",
       "}\n",
       "\n",
       "// Register the function which deals with the matplotlib target/channel.\n",
       "// The kernel may be null if the page has been refreshed.\n",
       "if (IPython.notebook.kernel != null) {\n",
       "    IPython.notebook.kernel.comm_manager.register_target('matplotlib', mpl.mpl_figure_comm);\n",
       "}\n"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/html": [
       "<div id='7caceb34-cecd-4eef-8628-ace14ac404a2'></div>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "hat_xs, zlist, us, delta = read_data()  \n",
    "dim = len(hat_xs)*3\n",
    "\n",
    "for n in range(1, 10000): \n",
    "    ##エッジ、大きな精度行列、係数ベクトルの作成##\n",
    "    edges, _ = make_edges(hat_xs, zlist)  #返す変数が2つになるので「_」で合わせる\n",
    "\n",
    "    for i in range(len(hat_xs)-1): #行動エッジの追加\n",
    "        edges.append(MotionEdge(i, i+1, hat_xs, us, delta))\n",
    "        \n",
    "    Omega = np.zeros((dim, dim))\n",
    "    xi = np.zeros(dim)\n",
    "    Omega[0:3, 0:3] += np.eye(3)*1000000\n",
    "\n",
    "    ##軌跡を動かす量（差分）の計算##\n",
    "    for e in edges:\n",
    "        add_edge(e, Omega, xi) \n",
    "\n",
    "    delta_xs = np.linalg.inv(Omega).dot(xi) \n",
    "    \n",
    "    ##推定値の更新##\n",
    "    for i in range(len(hat_xs)):\n",
    "        hat_xs[i] += delta_xs[i*3:(i+1)*3] \n",
    "        \n",
    "    ##終了判定##\n",
    "    diff = np.linalg.norm(delta_xs) \n",
    "    print(\"{}回目の繰り返し: {}\".format(n, diff))\n",
    "    if diff < 0.02:\n",
    "        draw(hat_xs, zlist, edges)\n",
    "        break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{1: [(0, (1, array([2.06692697, 0.1072544 ]))),\n",
       "  (1, (1, array([ 1.5096788 , -0.24896954]))),\n",
       "  (2, (1, array([ 0.70599846, -0.87980216]))),\n",
       "  (19, (1, array([5.88825157, 0.91219263]))),\n",
       "  (20, (1, array([5.49530319, 0.77184918]))),\n",
       "  (21, (1, array([4.22159569, 0.45413407]))),\n",
       "  (22, (1, array([3.30074879, 0.25687774]))),\n",
       "  (23, (1, array([3.81608544, 0.0631331 ]))),\n",
       "  (24, (1, array([ 2.43059288, -0.0623398 ]))),\n",
       "  (25, (1, array([ 1.78897471, -0.50307633]))),\n",
       "  (45, (1, array([4.77636009, 0.71157766]))),\n",
       "  (46, (1, array([5.66554641, 0.58590108]))),\n",
       "  (47, (1, array([5.09707481, 0.35610579]))),\n",
       "  (48, (1, array([3.69124295, 0.27880156]))),\n",
       "  (49, (1, array([3.33045759, 0.13861935]))),\n",
       "  (50, (1, array([ 2.38280131, -0.10618313]))),\n",
       "  (51, (1, array([ 1.30205637, -0.54669782])))],\n",
       " 2: [(2, (2, array([6.35152367, 0.92637676]))),\n",
       "  (3, (2, array([6.00363588, 0.69310826]))),\n",
       "  (4, (2, array([4.65862126, 0.51401244]))),\n",
       "  (5, (2, array([6.12674004, 0.41674825]))),\n",
       "  (7, (2, array([3.01613644, 0.0972639 ]))),\n",
       "  (8, (2, array([ 2.0954224 , -0.19231275]))),\n",
       "  (9, (2, array([ 1.16518676, -0.75597216]))),\n",
       "  (27, (2, array([4.77901829, 0.81640449]))),\n",
       "  (28, (2, array([4.78557814, 0.63363691]))),\n",
       "  (29, (2, array([4.14317093, 0.44662646]))),\n",
       "  (30, (2, array([3.4006121 , 0.27463498]))),\n",
       "  (31, (2, array([2.05439285, 0.10301851]))),\n",
       "  (32, (2, array([ 1.6002771 , -0.25714132]))),\n",
       "  (52, (2, array([5.66837674, 1.14252746]))),\n",
       "  (53, (2, array([5.29396509, 0.93183635]))),\n",
       "  (54, (2, array([5.64951946, 0.91856974]))),\n",
       "  (55, (2, array([3.82456132, 0.82276029]))),\n",
       "  (56, (2, array([3.25783928, 0.67890248]))),\n",
       "  (57, (2, array([2.85423399, 0.45264385]))),\n",
       "  (59, (2, array([1.27836978, 0.12584779]))),\n",
       "  (60, (2, array([ 0.57119859, -0.2058423 ])))],\n",
       " 3: [(5, (3, array([6.48499025, 0.98788833]))),\n",
       "  (6, (3, array([5.61188077, 0.90058745]))),\n",
       "  (7, (3, array([5.38452695, 0.82299452]))),\n",
       "  (8, (3, array([4.88553538, 0.6105311 ]))),\n",
       "  (9, (3, array([3.67833372, 0.33115254]))),\n",
       "  (10, (3, array([3.05729525, 0.06776183]))),\n",
       "  (11, (3, array([ 2.02572673, -0.15251464]))),\n",
       "  (12, (3, array([ 1.27155673, -0.44287556]))),\n",
       "  (29, (3, array([6.29506419, 1.06145852]))),\n",
       "  (30, (3, array([5.73987709, 0.90246793]))),\n",
       "  (31, (3, array([4.56528352, 0.91239991]))),\n",
       "  (32, (3, array([3.95717201, 0.64195357]))),\n",
       "  (33, (3, array([3.69698901, 0.57099778]))),\n",
       "  (34, (3, array([2.66320296, 0.4045528 ]))),\n",
       "  (35, (3, array([1.98854417, 0.30939456]))),\n",
       "  (36, (3, array([1.13573938, 0.2434932 ]))),\n",
       "  (57, (3, array([5.72479729, 1.01153046]))),\n",
       "  (58, (3, array([5.04484216, 0.81162724]))),\n",
       "  (60, (3, array([4.03278986, 0.60057391]))),\n",
       "  (61, (3, array([3.3721131 , 0.30583701])))],\n",
       " 0: [(11, (0, array([6.34874165, 0.71188524]))),\n",
       "  (12, (0, array([4.3969745 , 0.55064943]))),\n",
       "  (13, (0, array([4.10558318, 0.42142567]))),\n",
       "  (14, (0, array([4.23834577, 0.18840807]))),\n",
       "  (15, (0, array([ 3.66503475, -0.10418768]))),\n",
       "  (35, (0, array([5.20142771, 0.96092881]))),\n",
       "  (36, (0, array([5.94781563, 0.84710801]))),\n",
       "  (37, (0, array([5.18332535, 0.53290677]))),\n",
       "  (38, (0, array([3.79340415, 0.45877595]))),\n",
       "  (39, (0, array([3.49414359, 0.21704501]))),\n",
       "  (40, (0, array([ 2.85453919, -0.07417585]))),\n",
       "  (41, (0, array([ 2.11550557, -0.48056153])))],\n",
       " 5: [(13, (5, array([5.9053536 , 1.08585369]))),\n",
       "  (14, (5, array([5.15988352, 0.92534314]))),\n",
       "  (15, (5, array([3.89624764, 0.77295506]))),\n",
       "  (16, (5, array([3.65127407, 0.54401152]))),\n",
       "  (17, (5, array([3.59447691, 0.3360014 ]))),\n",
       "  (18, (5, array([2.47463663, 0.10333659]))),\n",
       "  (19, (5, array([ 1.68357465, -0.34946898]))),\n",
       "  (20, (5, array([ 0.95468651, -0.94248429]))),\n",
       "  (37, (5, array([6.13444875, 1.13216029]))),\n",
       "  (38, (5, array([6.25837274, 1.03400009]))),\n",
       "  (39, (5, array([5.46098097, 0.7755304 ]))),\n",
       "  (40, (5, array([5.32247052, 0.68643632]))),\n",
       "  (41, (5, array([3.79781786, 0.61967663]))),\n",
       "  (42, (5, array([3.34925376, 0.34576283]))),\n",
       "  (43, (5, array([3.09698225, 0.08114893]))),\n",
       "  (44, (5, array([ 1.77788169, -0.21186547]))),\n",
       "  (45, (5, array([ 1.24991253, -0.66586217])))]}"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "_, zlist_landmark = make_edges(hat_xs, zlist) \n",
    "zlist_landmark"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "class MapEdge: ###graphbasedslam_2d_sensor_mapedge\n",
    "    def __init__(self, t, z, head_t, head_z, xs, sensor_noise_rate=[0.14, 0.05]):  #センサの雑音モデルを削除\n",
    "        self.x = xs[t]\n",
    "        self.z = z\n",
    "        \n",
    "        self.m = self.x[0:2] + np.array([z[0]*math.cos(self.x[2] + z[1]), z[0]*math.sin(self.x[2] + z[1])]).T  #3行目削除\n",
    "            \n",
    "        ##精度行列の計算## \n",
    "        Q1 = np.diag([(self.z[0]*sensor_noise_rate[0])**2, sensor_noise_rate[1]**2]) #ψの分散を削除\n",
    "        \n",
    "        s1 = math.sin(self.x[2] + self.z[1]) \n",
    "        c1 = math.cos(self.x[2] + self.z[1])\n",
    "        R = np.array([[-c1, self.z[0]*s1],\n",
    "                                [-s1,-self.z[0]*c1]]) #3行目、3列目を削除\n",
    "        \n",
    "        self.Omega = np.linalg.inv(R.dot(Q1).dot(R.T)) #2x2行列になる\n",
    "        self.xi = self.Omega.dot(self.m)   #2次元ベクトルになる"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/javascript": [
       "/* Put everything inside the global mpl namespace */\n",
       "window.mpl = {};\n",
       "\n",
       "\n",
       "mpl.get_websocket_type = function() {\n",
       "    if (typeof(WebSocket) !== 'undefined') {\n",
       "        return WebSocket;\n",
       "    } else if (typeof(MozWebSocket) !== 'undefined') {\n",
       "        return MozWebSocket;\n",
       "    } else {\n",
       "        alert('Your browser does not have WebSocket support. ' +\n",
       "              'Please try Chrome, Safari or Firefox ≥ 6. ' +\n",
       "              'Firefox 4 and 5 are also supported but you ' +\n",
       "              'have to enable WebSockets in about:config.');\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure = function(figure_id, websocket, ondownload, parent_element) {\n",
       "    this.id = figure_id;\n",
       "\n",
       "    this.ws = websocket;\n",
       "\n",
       "    this.supports_binary = (this.ws.binaryType != undefined);\n",
       "\n",
       "    if (!this.supports_binary) {\n",
       "        var warnings = document.getElementById(\"mpl-warnings\");\n",
       "        if (warnings) {\n",
       "            warnings.style.display = 'block';\n",
       "            warnings.textContent = (\n",
       "                \"This browser does not support binary websocket messages. \" +\n",
       "                    \"Performance may be slow.\");\n",
       "        }\n",
       "    }\n",
       "\n",
       "    this.imageObj = new Image();\n",
       "\n",
       "    this.context = undefined;\n",
       "    this.message = undefined;\n",
       "    this.canvas = undefined;\n",
       "    this.rubberband_canvas = undefined;\n",
       "    this.rubberband_context = undefined;\n",
       "    this.format_dropdown = undefined;\n",
       "\n",
       "    this.image_mode = 'full';\n",
       "\n",
       "    this.root = $('<div/>');\n",
       "    this._root_extra_style(this.root)\n",
       "    this.root.attr('style', 'display: inline-block');\n",
       "\n",
       "    $(parent_element).append(this.root);\n",
       "\n",
       "    this._init_header(this);\n",
       "    this._init_canvas(this);\n",
       "    this._init_toolbar(this);\n",
       "\n",
       "    var fig = this;\n",
       "\n",
       "    this.waiting = false;\n",
       "\n",
       "    this.ws.onopen =  function () {\n",
       "            fig.send_message(\"supports_binary\", {value: fig.supports_binary});\n",
       "            fig.send_message(\"send_image_mode\", {});\n",
       "            if (mpl.ratio != 1) {\n",
       "                fig.send_message(\"set_dpi_ratio\", {'dpi_ratio': mpl.ratio});\n",
       "            }\n",
       "            fig.send_message(\"refresh\", {});\n",
       "        }\n",
       "\n",
       "    this.imageObj.onload = function() {\n",
       "            if (fig.image_mode == 'full') {\n",
       "                // Full images could contain transparency (where diff images\n",
       "                // almost always do), so we need to clear the canvas so that\n",
       "                // there is no ghosting.\n",
       "                fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n",
       "            }\n",
       "            fig.context.drawImage(fig.imageObj, 0, 0);\n",
       "        };\n",
       "\n",
       "    this.imageObj.onunload = function() {\n",
       "        fig.ws.close();\n",
       "    }\n",
       "\n",
       "    this.ws.onmessage = this._make_on_message_function(this);\n",
       "\n",
       "    this.ondownload = ondownload;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_header = function() {\n",
       "    var titlebar = $(\n",
       "        '<div class=\"ui-dialog-titlebar ui-widget-header ui-corner-all ' +\n",
       "        'ui-helper-clearfix\"/>');\n",
       "    var titletext = $(\n",
       "        '<div class=\"ui-dialog-title\" style=\"width: 100%; ' +\n",
       "        'text-align: center; padding: 3px;\"/>');\n",
       "    titlebar.append(titletext)\n",
       "    this.root.append(titlebar);\n",
       "    this.header = titletext[0];\n",
       "}\n",
       "\n",
       "\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_canvas = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var canvas_div = $('<div/>');\n",
       "\n",
       "    canvas_div.attr('style', 'position: relative; clear: both; outline: 0');\n",
       "\n",
       "    function canvas_keyboard_event(event) {\n",
       "        return fig.key_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    canvas_div.keydown('key_press', canvas_keyboard_event);\n",
       "    canvas_div.keyup('key_release', canvas_keyboard_event);\n",
       "    this.canvas_div = canvas_div\n",
       "    this._canvas_extra_style(canvas_div)\n",
       "    this.root.append(canvas_div);\n",
       "\n",
       "    var canvas = $('<canvas/>');\n",
       "    canvas.addClass('mpl-canvas');\n",
       "    canvas.attr('style', \"left: 0; top: 0; z-index: 0; outline: 0\")\n",
       "\n",
       "    this.canvas = canvas[0];\n",
       "    this.context = canvas[0].getContext(\"2d\");\n",
       "\n",
       "    var backingStore = this.context.backingStorePixelRatio ||\n",
       "\tthis.context.webkitBackingStorePixelRatio ||\n",
       "\tthis.context.mozBackingStorePixelRatio ||\n",
       "\tthis.context.msBackingStorePixelRatio ||\n",
       "\tthis.context.oBackingStorePixelRatio ||\n",
       "\tthis.context.backingStorePixelRatio || 1;\n",
       "\n",
       "    mpl.ratio = (window.devicePixelRatio || 1) / backingStore;\n",
       "\n",
       "    var rubberband = $('<canvas/>');\n",
       "    rubberband.attr('style', \"position: absolute; left: 0; top: 0; z-index: 1;\")\n",
       "\n",
       "    var pass_mouse_events = true;\n",
       "\n",
       "    canvas_div.resizable({\n",
       "        start: function(event, ui) {\n",
       "            pass_mouse_events = false;\n",
       "        },\n",
       "        resize: function(event, ui) {\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "        stop: function(event, ui) {\n",
       "            pass_mouse_events = true;\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "    });\n",
       "\n",
       "    function mouse_event_fn(event) {\n",
       "        if (pass_mouse_events)\n",
       "            return fig.mouse_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    rubberband.mousedown('button_press', mouse_event_fn);\n",
       "    rubberband.mouseup('button_release', mouse_event_fn);\n",
       "    // Throttle sequential mouse events to 1 every 20ms.\n",
       "    rubberband.mousemove('motion_notify', mouse_event_fn);\n",
       "\n",
       "    rubberband.mouseenter('figure_enter', mouse_event_fn);\n",
       "    rubberband.mouseleave('figure_leave', mouse_event_fn);\n",
       "\n",
       "    canvas_div.on(\"wheel\", function (event) {\n",
       "        event = event.originalEvent;\n",
       "        event['data'] = 'scroll'\n",
       "        if (event.deltaY < 0) {\n",
       "            event.step = 1;\n",
       "        } else {\n",
       "            event.step = -1;\n",
       "        }\n",
       "        mouse_event_fn(event);\n",
       "    });\n",
       "\n",
       "    canvas_div.append(canvas);\n",
       "    canvas_div.append(rubberband);\n",
       "\n",
       "    this.rubberband = rubberband;\n",
       "    this.rubberband_canvas = rubberband[0];\n",
       "    this.rubberband_context = rubberband[0].getContext(\"2d\");\n",
       "    this.rubberband_context.strokeStyle = \"#000000\";\n",
       "\n",
       "    this._resize_canvas = function(width, height) {\n",
       "        // Keep the size of the canvas, canvas container, and rubber band\n",
       "        // canvas in synch.\n",
       "        canvas_div.css('width', width)\n",
       "        canvas_div.css('height', height)\n",
       "\n",
       "        canvas.attr('width', width * mpl.ratio);\n",
       "        canvas.attr('height', height * mpl.ratio);\n",
       "        canvas.attr('style', 'width: ' + width + 'px; height: ' + height + 'px;');\n",
       "\n",
       "        rubberband.attr('width', width);\n",
       "        rubberband.attr('height', height);\n",
       "    }\n",
       "\n",
       "    // Set the figure to an initial 600x600px, this will subsequently be updated\n",
       "    // upon first draw.\n",
       "    this._resize_canvas(600, 600);\n",
       "\n",
       "    // Disable right mouse context menu.\n",
       "    $(this.rubberband_canvas).bind(\"contextmenu\",function(e){\n",
       "        return false;\n",
       "    });\n",
       "\n",
       "    function set_focus () {\n",
       "        canvas.focus();\n",
       "        canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    window.setTimeout(set_focus, 100);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>');\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items) {\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) {\n",
       "            // put a spacer in here.\n",
       "            continue;\n",
       "        }\n",
       "        var button = $('<button/>');\n",
       "        button.addClass('ui-button ui-widget ui-state-default ui-corner-all ' +\n",
       "                        'ui-button-icon-only');\n",
       "        button.attr('role', 'button');\n",
       "        button.attr('aria-disabled', 'false');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "\n",
       "        var icon_img = $('<span/>');\n",
       "        icon_img.addClass('ui-button-icon-primary ui-icon');\n",
       "        icon_img.addClass(image);\n",
       "        icon_img.addClass('ui-corner-all');\n",
       "\n",
       "        var tooltip_span = $('<span/>');\n",
       "        tooltip_span.addClass('ui-button-text');\n",
       "        tooltip_span.html(tooltip);\n",
       "\n",
       "        button.append(icon_img);\n",
       "        button.append(tooltip_span);\n",
       "\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    var fmt_picker_span = $('<span/>');\n",
       "\n",
       "    var fmt_picker = $('<select/>');\n",
       "    fmt_picker.addClass('mpl-toolbar-option ui-widget ui-widget-content');\n",
       "    fmt_picker_span.append(fmt_picker);\n",
       "    nav_element.append(fmt_picker_span);\n",
       "    this.format_dropdown = fmt_picker[0];\n",
       "\n",
       "    for (var ind in mpl.extensions) {\n",
       "        var fmt = mpl.extensions[ind];\n",
       "        var option = $(\n",
       "            '<option/>', {selected: fmt === mpl.default_extension}).html(fmt);\n",
       "        fmt_picker.append(option);\n",
       "    }\n",
       "\n",
       "    // Add hover states to the ui-buttons\n",
       "    $( \".ui-button\" ).hover(\n",
       "        function() { $(this).addClass(\"ui-state-hover\");},\n",
       "        function() { $(this).removeClass(\"ui-state-hover\");}\n",
       "    );\n",
       "\n",
       "    var status_bar = $('<span class=\"mpl-message\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.request_resize = function(x_pixels, y_pixels) {\n",
       "    // Request matplotlib to resize the figure. Matplotlib will then trigger a resize in the client,\n",
       "    // which will in turn request a refresh of the image.\n",
       "    this.send_message('resize', {'width': x_pixels, 'height': y_pixels});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_message = function(type, properties) {\n",
       "    properties['type'] = type;\n",
       "    properties['figure_id'] = this.id;\n",
       "    this.ws.send(JSON.stringify(properties));\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_draw_message = function() {\n",
       "    if (!this.waiting) {\n",
       "        this.waiting = true;\n",
       "        this.ws.send(JSON.stringify({type: \"draw\", figure_id: this.id}));\n",
       "    }\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    var format_dropdown = fig.format_dropdown;\n",
       "    var format = format_dropdown.options[format_dropdown.selectedIndex].value;\n",
       "    fig.ondownload(fig, format);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_resize = function(fig, msg) {\n",
       "    var size = msg['size'];\n",
       "    if (size[0] != fig.canvas.width || size[1] != fig.canvas.height) {\n",
       "        fig._resize_canvas(size[0], size[1]);\n",
       "        fig.send_message(\"refresh\", {});\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_rubberband = function(fig, msg) {\n",
       "    var x0 = msg['x0'] / mpl.ratio;\n",
       "    var y0 = (fig.canvas.height - msg['y0']) / mpl.ratio;\n",
       "    var x1 = msg['x1'] / mpl.ratio;\n",
       "    var y1 = (fig.canvas.height - msg['y1']) / mpl.ratio;\n",
       "    x0 = Math.floor(x0) + 0.5;\n",
       "    y0 = Math.floor(y0) + 0.5;\n",
       "    x1 = Math.floor(x1) + 0.5;\n",
       "    y1 = Math.floor(y1) + 0.5;\n",
       "    var min_x = Math.min(x0, x1);\n",
       "    var min_y = Math.min(y0, y1);\n",
       "    var width = Math.abs(x1 - x0);\n",
       "    var height = Math.abs(y1 - y0);\n",
       "\n",
       "    fig.rubberband_context.clearRect(\n",
       "        0, 0, fig.canvas.width / mpl.ratio, fig.canvas.height / mpl.ratio);\n",
       "\n",
       "    fig.rubberband_context.strokeRect(min_x, min_y, width, height);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_figure_label = function(fig, msg) {\n",
       "    // Updates the figure title.\n",
       "    fig.header.textContent = msg['label'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_cursor = function(fig, msg) {\n",
       "    var cursor = msg['cursor'];\n",
       "    switch(cursor)\n",
       "    {\n",
       "    case 0:\n",
       "        cursor = 'pointer';\n",
       "        break;\n",
       "    case 1:\n",
       "        cursor = 'default';\n",
       "        break;\n",
       "    case 2:\n",
       "        cursor = 'crosshair';\n",
       "        break;\n",
       "    case 3:\n",
       "        cursor = 'move';\n",
       "        break;\n",
       "    }\n",
       "    fig.rubberband_canvas.style.cursor = cursor;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_message = function(fig, msg) {\n",
       "    fig.message.textContent = msg['message'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_draw = function(fig, msg) {\n",
       "    // Request the server to send over a new figure.\n",
       "    fig.send_draw_message();\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_image_mode = function(fig, msg) {\n",
       "    fig.image_mode = msg['mode'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Called whenever the canvas gets updated.\n",
       "    this.send_message(\"ack\", {});\n",
       "}\n",
       "\n",
       "// A function to construct a web socket function for onmessage handling.\n",
       "// Called in the figure constructor.\n",
       "mpl.figure.prototype._make_on_message_function = function(fig) {\n",
       "    return function socket_on_message(evt) {\n",
       "        if (evt.data instanceof Blob) {\n",
       "            /* FIXME: We get \"Resource interpreted as Image but\n",
       "             * transferred with MIME type text/plain:\" errors on\n",
       "             * Chrome.  But how to set the MIME type?  It doesn't seem\n",
       "             * to be part of the websocket stream */\n",
       "            evt.data.type = \"image/png\";\n",
       "\n",
       "            /* Free the memory for the previous frames */\n",
       "            if (fig.imageObj.src) {\n",
       "                (window.URL || window.webkitURL).revokeObjectURL(\n",
       "                    fig.imageObj.src);\n",
       "            }\n",
       "\n",
       "            fig.imageObj.src = (window.URL || window.webkitURL).createObjectURL(\n",
       "                evt.data);\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "        else if (typeof evt.data === 'string' && evt.data.slice(0, 21) == \"data:image/png;base64\") {\n",
       "            fig.imageObj.src = evt.data;\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        var msg = JSON.parse(evt.data);\n",
       "        var msg_type = msg['type'];\n",
       "\n",
       "        // Call the  \"handle_{type}\" callback, which takes\n",
       "        // the figure and JSON message as its only arguments.\n",
       "        try {\n",
       "            var callback = fig[\"handle_\" + msg_type];\n",
       "        } catch (e) {\n",
       "            console.log(\"No handler for the '\" + msg_type + \"' message type: \", msg);\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        if (callback) {\n",
       "            try {\n",
       "                // console.log(\"Handling '\" + msg_type + \"' message: \", msg);\n",
       "                callback(fig, msg);\n",
       "            } catch (e) {\n",
       "                console.log(\"Exception inside the 'handler_\" + msg_type + \"' callback:\", e, e.stack, msg);\n",
       "            }\n",
       "        }\n",
       "    };\n",
       "}\n",
       "\n",
       "// from http://stackoverflow.com/questions/1114465/getting-mouse-location-in-canvas\n",
       "mpl.findpos = function(e) {\n",
       "    //this section is from http://www.quirksmode.org/js/events_properties.html\n",
       "    var targ;\n",
       "    if (!e)\n",
       "        e = window.event;\n",
       "    if (e.target)\n",
       "        targ = e.target;\n",
       "    else if (e.srcElement)\n",
       "        targ = e.srcElement;\n",
       "    if (targ.nodeType == 3) // defeat Safari bug\n",
       "        targ = targ.parentNode;\n",
       "\n",
       "    // jQuery normalizes the pageX and pageY\n",
       "    // pageX,Y are the mouse positions relative to the document\n",
       "    // offset() returns the position of the element relative to the document\n",
       "    var x = e.pageX - $(targ).offset().left;\n",
       "    var y = e.pageY - $(targ).offset().top;\n",
       "\n",
       "    return {\"x\": x, \"y\": y};\n",
       "};\n",
       "\n",
       "/*\n",
       " * return a copy of an object with only non-object keys\n",
       " * we need this to avoid circular references\n",
       " * http://stackoverflow.com/a/24161582/3208463\n",
       " */\n",
       "function simpleKeys (original) {\n",
       "  return Object.keys(original).reduce(function (obj, key) {\n",
       "    if (typeof original[key] !== 'object')\n",
       "        obj[key] = original[key]\n",
       "    return obj;\n",
       "  }, {});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.mouse_event = function(event, name) {\n",
       "    var canvas_pos = mpl.findpos(event)\n",
       "\n",
       "    if (name === 'button_press')\n",
       "    {\n",
       "        this.canvas.focus();\n",
       "        this.canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    var x = canvas_pos.x * mpl.ratio;\n",
       "    var y = canvas_pos.y * mpl.ratio;\n",
       "\n",
       "    this.send_message(name, {x: x, y: y, button: event.button,\n",
       "                             step: event.step,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "\n",
       "    /* This prevents the web browser from automatically changing to\n",
       "     * the text insertion cursor when the button is pressed.  We want\n",
       "     * to control all of the cursor setting manually through the\n",
       "     * 'cursor' event from matplotlib */\n",
       "    event.preventDefault();\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    // Handle any extra behaviour associated with a key event\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.key_event = function(event, name) {\n",
       "\n",
       "    // Prevent repeat events\n",
       "    if (name == 'key_press')\n",
       "    {\n",
       "        if (event.which === this._key)\n",
       "            return;\n",
       "        else\n",
       "            this._key = event.which;\n",
       "    }\n",
       "    if (name == 'key_release')\n",
       "        this._key = null;\n",
       "\n",
       "    var value = '';\n",
       "    if (event.ctrlKey && event.which != 17)\n",
       "        value += \"ctrl+\";\n",
       "    if (event.altKey && event.which != 18)\n",
       "        value += \"alt+\";\n",
       "    if (event.shiftKey && event.which != 16)\n",
       "        value += \"shift+\";\n",
       "\n",
       "    value += 'k';\n",
       "    value += event.which.toString();\n",
       "\n",
       "    this._key_event_extra(event, name);\n",
       "\n",
       "    this.send_message(name, {key: value,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onclick = function(name) {\n",
       "    if (name == 'download') {\n",
       "        this.handle_save(this, null);\n",
       "    } else {\n",
       "        this.send_message(\"toolbar_button\", {name: name});\n",
       "    }\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onmouseover = function(tooltip) {\n",
       "    this.message.textContent = tooltip;\n",
       "};\n",
       "mpl.toolbar_items = [[\"Home\", \"Reset original view\", \"fa fa-home icon-home\", \"home\"], [\"Back\", \"Back to previous view\", \"fa fa-arrow-left icon-arrow-left\", \"back\"], [\"Forward\", \"Forward to next view\", \"fa fa-arrow-right icon-arrow-right\", \"forward\"], [\"\", \"\", \"\", \"\"], [\"Pan\", \"Pan axes with left mouse, zoom with right\", \"fa fa-arrows icon-move\", \"pan\"], [\"Zoom\", \"Zoom to rectangle\", \"fa fa-square-o icon-check-empty\", \"zoom\"], [\"\", \"\", \"\", \"\"], [\"Download\", \"Download plot\", \"fa fa-floppy-o icon-save\", \"download\"]];\n",
       "\n",
       "mpl.extensions = [\"eps\", \"jpeg\", \"pdf\", \"png\", \"ps\", \"raw\", \"svg\", \"tif\"];\n",
       "\n",
       "mpl.default_extension = \"png\";var comm_websocket_adapter = function(comm) {\n",
       "    // Create a \"websocket\"-like object which calls the given IPython comm\n",
       "    // object with the appropriate methods. Currently this is a non binary\n",
       "    // socket, so there is still some room for performance tuning.\n",
       "    var ws = {};\n",
       "\n",
       "    ws.close = function() {\n",
       "        comm.close()\n",
       "    };\n",
       "    ws.send = function(m) {\n",
       "        //console.log('sending', m);\n",
       "        comm.send(m);\n",
       "    };\n",
       "    // Register the callback with on_msg.\n",
       "    comm.on_msg(function(msg) {\n",
       "        //console.log('receiving', msg['content']['data'], msg);\n",
       "        // Pass the mpl event to the overridden (by mpl) onmessage function.\n",
       "        ws.onmessage(msg['content']['data'])\n",
       "    });\n",
       "    return ws;\n",
       "}\n",
       "\n",
       "mpl.mpl_figure_comm = function(comm, msg) {\n",
       "    // This is the function which gets called when the mpl process\n",
       "    // starts-up an IPython Comm through the \"matplotlib\" channel.\n",
       "\n",
       "    var id = msg.content.data.id;\n",
       "    // Get hold of the div created by the display call when the Comm\n",
       "    // socket was opened in Python.\n",
       "    var element = $(\"#\" + id);\n",
       "    var ws_proxy = comm_websocket_adapter(comm)\n",
       "\n",
       "    function ondownload(figure, format) {\n",
       "        window.open(figure.imageObj.src);\n",
       "    }\n",
       "\n",
       "    var fig = new mpl.figure(id, ws_proxy,\n",
       "                           ondownload,\n",
       "                           element.get(0));\n",
       "\n",
       "    // Call onopen now - mpl needs it, as it is assuming we've passed it a real\n",
       "    // web socket which is closed, not our websocket->open comm proxy.\n",
       "    ws_proxy.onopen();\n",
       "\n",
       "    fig.parent_element = element.get(0);\n",
       "    fig.cell_info = mpl.find_output_cell(\"<div id='\" + id + \"'></div>\");\n",
       "    if (!fig.cell_info) {\n",
       "        console.error(\"Failed to find cell for figure\", id, fig);\n",
       "        return;\n",
       "    }\n",
       "\n",
       "    var output_index = fig.cell_info[2]\n",
       "    var cell = fig.cell_info[0];\n",
       "\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.handle_close = function(fig, msg) {\n",
       "    var width = fig.canvas.width/mpl.ratio\n",
       "    fig.root.unbind('remove')\n",
       "\n",
       "    // Update the output cell to use the data from the current canvas.\n",
       "    fig.push_to_output();\n",
       "    var dataURL = fig.canvas.toDataURL();\n",
       "    // Re-enable the keyboard manager in IPython - without this line, in FF,\n",
       "    // the notebook keyboard shortcuts fail.\n",
       "    IPython.keyboard_manager.enable()\n",
       "    $(fig.parent_element).html('<img src=\"' + dataURL + '\" width=\"' + width + '\">');\n",
       "    fig.close_ws(fig, msg);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.close_ws = function(fig, msg){\n",
       "    fig.send_message('closing', msg);\n",
       "    // fig.ws.close()\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.push_to_output = function(remove_interactive) {\n",
       "    // Turn the data on the canvas into data in the output cell.\n",
       "    var width = this.canvas.width/mpl.ratio\n",
       "    var dataURL = this.canvas.toDataURL();\n",
       "    this.cell_info[1]['text/html'] = '<img src=\"' + dataURL + '\" width=\"' + width + '\">';\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Tell IPython that the notebook contents must change.\n",
       "    IPython.notebook.set_dirty(true);\n",
       "    this.send_message(\"ack\", {});\n",
       "    var fig = this;\n",
       "    // Wait a second, then push the new image to the DOM so\n",
       "    // that it is saved nicely (might be nice to debounce this).\n",
       "    setTimeout(function () { fig.push_to_output() }, 1000);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>');\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items){\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) { continue; };\n",
       "\n",
       "        var button = $('<button class=\"btn btn-default\" href=\"#\" title=\"' + name + '\"><i class=\"fa ' + image + ' fa-lg\"></i></button>');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    // Add the status bar.\n",
       "    var status_bar = $('<span class=\"mpl-message\" style=\"text-align:right; float: right;\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "\n",
       "    // Add the close button to the window.\n",
       "    var buttongrp = $('<div class=\"btn-group inline pull-right\"></div>');\n",
       "    var button = $('<button class=\"btn btn-mini btn-primary\" href=\"#\" title=\"Stop Interaction\"><i class=\"fa fa-power-off icon-remove icon-large\"></i></button>');\n",
       "    button.click(function (evt) { fig.handle_close(fig, {}); } );\n",
       "    button.mouseover('Stop Interaction', toolbar_mouse_event);\n",
       "    buttongrp.append(button);\n",
       "    var titlebar = this.root.find($('.ui-dialog-titlebar'));\n",
       "    titlebar.prepend(buttongrp);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(el){\n",
       "    var fig = this\n",
       "    el.on(\"remove\", function(){\n",
       "\tfig.close_ws(fig, {});\n",
       "    });\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(el){\n",
       "    // this is important to make the div 'focusable\n",
       "    el.attr('tabindex', 0)\n",
       "    // reach out to IPython and tell the keyboard manager to turn it's self\n",
       "    // off when our div gets focus\n",
       "\n",
       "    // location in version 3\n",
       "    if (IPython.notebook.keyboard_manager) {\n",
       "        IPython.notebook.keyboard_manager.register_events(el);\n",
       "    }\n",
       "    else {\n",
       "        // location in version 2\n",
       "        IPython.keyboard_manager.register_events(el);\n",
       "    }\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    var manager = IPython.notebook.keyboard_manager;\n",
       "    if (!manager)\n",
       "        manager = IPython.keyboard_manager;\n",
       "\n",
       "    // Check for shift+enter\n",
       "    if (event.shiftKey && event.which == 13) {\n",
       "        this.canvas_div.blur();\n",
       "        // select the cell after this one\n",
       "        var index = IPython.notebook.find_cell_index(this.cell_info[0]);\n",
       "        IPython.notebook.select(index + 1);\n",
       "    }\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    fig.ondownload(fig, null);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.find_output_cell = function(html_output) {\n",
       "    // Return the cell and output element which can be found *uniquely* in the notebook.\n",
       "    // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n",
       "    // IPython event is triggered only after the cells have been serialised, which for\n",
       "    // our purposes (turning an active figure into a static one), is too late.\n",
       "    var cells = IPython.notebook.get_cells();\n",
       "    var ncells = cells.length;\n",
       "    for (var i=0; i<ncells; i++) {\n",
       "        var cell = cells[i];\n",
       "        if (cell.cell_type === 'code'){\n",
       "            for (var j=0; j<cell.output_area.outputs.length; j++) {\n",
       "                var data = cell.output_area.outputs[j];\n",
       "                if (data.data) {\n",
       "                    // IPython >= 3 moved mimebundle to data attribute of output\n",
       "                    data = data.data;\n",
       "                }\n",
       "                if (data['text/html'] == html_output) {\n",
       "                    return [cell, data, j];\n",
       "                }\n",
       "            }\n",
       "        }\n",
       "    }\n",
       "}\n",
       "\n",
       "// Register the function which deals with the matplotlib target/channel.\n",
       "// The kernel may be null if the page has been refreshed.\n",
       "if (IPython.notebook.kernel != null) {\n",
       "    IPython.notebook.kernel.comm_manager.register_target('matplotlib', mpl.mpl_figure_comm);\n",
       "}\n"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/html": [
       "<div id='e7b7345d-9b2a-4ce5-a674-3a5fb8d973c7'></div>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "ms = {}###graphbasedslam_2d_sensor_mapexec\n",
    "for landmark_id in zlist_landmark:\n",
    "    edges = []\n",
    "    head_z = zlist_landmark[landmark_id][0]\n",
    "    for z in zlist_landmark[landmark_id]:\n",
    "        edges.append(MapEdge(z[0], z[1][1], head_z[0], head_z[1][1], hat_xs))\n",
    "        \n",
    "    Omega = np.zeros((2,2)) #2x2に\n",
    "    xi = np.zeros(2)                 #2次元に\n",
    "    for e in edges:\n",
    "        Omega += e.Omega\n",
    "        xi += e.xi\n",
    "        \n",
    "    ms[landmark_id] = np.mean([e.m for e in edges], axis=0)\n",
    "    \n",
    "draw(hat_xs, zlist, edges, ms)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{1: array([ 1.7180129 , -3.08128803]),\n",
       " 2: array([ 7.1059205 , -5.44003902]),\n",
       " 3: array([ 8.92807734, -2.54635957]),\n",
       " 0: array([7.81897946, 1.66669421]),\n",
       " 5: array([4.49762136, 1.38200136])}"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "actual_pos = [(-4,2), (2,-3), (3,3), (0,4), (1,1), (-3,-1)]\n",
    "ms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "from decimal import Decimal, ROUND_HALF_UP\n",
    "def distance(landmark_id, ref_id1, ref_id2, ms):\n",
    "    m, ref_m1, ref_m2 = ms[landmark_id], ms[ref_id1], ms[ref_id2]\n",
    "    d = math.sqrt( (m[0]-ref_m1[0])**2 + (m[1]-ref_m1[1])**2 )\n",
    "    ang = math.atan2(m[1] - ref_m1[1], m[0] - ref_m1[0]) - math.atan2(ref_m2[1] - ref_m1[1], ref_m2[0] - ref_m1[0])\n",
    "    return Decimal(d).quantize(Decimal(\"0.01\"), rounding=ROUND_HALF_UP), Decimal(ang/math.pi*180).quantize(Decimal(\"0.1\"), rounding=ROUND_HALF_UP)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1 7.81 0.0 7.73 0.0 -0.08 0.0\n",
      "2 7.07 47.9 7.14 46.4 0.07 -1.5\n",
      "3 4.47 66.4 4.36 66.9 -0.11 0.5\n"
     ]
    },
    {
     "ename": "KeyError",
     "evalue": "4",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyError\u001b[0m                                  Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-14-b4b4ece64c40>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;36m6\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 2\u001b[0;31m     \u001b[0md\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mang\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdistance\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mms\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      3\u001b[0m     \u001b[0mtrue_d\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtrue_ang\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdistance\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mactual_pos\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      4\u001b[0m     \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtrue_d\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtrue_ang\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0md\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mang\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0md\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mtrue_d\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mang\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mtrue_ang\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m<ipython-input-13-070bb77b3c2c>\u001b[0m in \u001b[0;36mdistance\u001b[0;34m(landmark_id, ref_id1, ref_id2, ms)\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0;32mfrom\u001b[0m \u001b[0mdecimal\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mDecimal\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mROUND_HALF_UP\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      2\u001b[0m \u001b[0;32mdef\u001b[0m \u001b[0mdistance\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlandmark_id\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mref_id1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mref_id2\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mms\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 3\u001b[0;31m     \u001b[0mm\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mref_m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mref_m2\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mms\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mlandmark_id\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mms\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mref_id1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mms\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mref_id2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      4\u001b[0m     \u001b[0md\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msqrt\u001b[0m\u001b[0;34m(\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mm\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mref_m1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m**\u001b[0m\u001b[0;36m2\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0mm\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mref_m1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m**\u001b[0m\u001b[0;36m2\u001b[0m \u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      5\u001b[0m     \u001b[0mang\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0matan2\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mm\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mref_m1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mm\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mref_m1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mmath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0matan2\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mref_m2\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mref_m1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mref_m2\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mref_m1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mKeyError\u001b[0m: 4"
     ]
    }
   ],
   "source": [
    "for i in range(1,6):\n",
    "    d, ang = distance(i, 0, 1, ms)\n",
    "    true_d, true_ang = distance(i, 0, 1, actual_pos)\n",
    "    print(i, true_d, true_ang, d, ang, d - true_d, ang - true_ang)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
